#include "pipo.h"

/*************************************************************************
 return 0 or 1 depending on logical condition
*************************************************************************/

int16_t getVal(uint8_t idx, uint8_t level=0); //required fwd declaration

uint8_t getLogicalCondition(uint8_t idx, uint8_t level=0)
{
    if(level>3) return false; //prevent recursive loop going too deep

    if(!idx || idx>=IN_LOGICAL_COND_LEN) return 0;
    LogicalCondition * lc = &gVars.lConditions[idx];
    if(!lc->func) return 0;


    int8_t a = lc->v1;
    int8_t b = lc->v2;
    int16_t x = 0;
    int16_t y = 0;

    // init values only if needed
    uint8_t s = LC_STATE(lc->func);

    if(s == LC_VOFS)
    {
        x = getVal(lc->v1-1);
        y = lc->v2;
    }
    else if(s == LC_VCOMP)
    {
        x = getVal(lc->v1, level+1);
        y = getVal(lc->v2, level+1);
    }

    switch (lc->func) {
    case (LC_VPOS):
        return (x>y);
        break;
    case (LC_VNEG):
        return (x<y);
        break;
    case (LC_APOS):
        return (abs(x)>y);
        break;
    case (LC_ANEG):
        return (abs(x)<y);
        break;

    case (LC_AND):
        return (getVal(a,level+1) && getVal(b,level+1));
        break;
    case (LC_OR):
        return (getVal(a,level+1) || getVal(b,level+1));
        break;
    case (LC_XOR):
        return (getVal(a,level+1) ^ getVal(b,level+1));
        break;

    case (LC_EQUAL):
        return (x==y);
        break;
    case (LC_NEQUAL):
        return (x!=y);
        break;
    case (LC_GREATER):
        return (x>y);
        break;
    case (LC_LESS):
        return (x<y);
        break;
    case (LC_EGREATER):
        return (x>=y);
        break;
    case (LC_ELESS):
        return (x<=y);
        break;
    default:
        return false;
        break;
    }
}

/*************************************************************************
 return value
*************************************************************************/
int16_t getVal(uint8_t idx, uint8_t level)
{
    if(idx==IN_VAL_OFF) return 0;
    else if(idx==IN_VAL_ON) return RESX;
    else if(idx<(IN_PWM_IN_DATA+IN_PWM_IN_DATA_LEN))   return pwm_in_val[idx-IN_PWM_IN_DATA];
    else if(idx<(IN_SENSOR_DATA+IN_SENSOR_DATA_LEN))   return sensor_data_int[idx-IN_SENSOR_DATA];
    else if(idx<(IN_ATTITUDE+IN_ATTITUDE_LEN))         return round(attitude[idx-IN_ATTITUDE]*RESX/M_PI);  // attitude in RADIAN -> pi -> RESX
    else if(idx<(IN_VELOCITY+IN_VELOCITY_LEN))         return 0; //no velocity data yet
    else if(idx<(IN_POSITION+IN_POSITION_LEN))         return 0; //no position data yet
    else if(idx<(IN_OUTPUTS+IN_OUTPUTS_LEN))           return outval[idx-IN_OUTPUTS];
    else if(idx<(IN_LOGICAL_COND+IN_LOGICAL_COND_LEN)) return getLogicalCondition(idx-IN_LOGICAL_COND, level);
    else if(idx<(IN_PID+IN_PID_LEN))                   return pidRefs[idx-IN_PID].val;
    return 0;
}


/*************************************************************************
 update multiplexer array
 takes inputs - operation - outputs
 dest = (source*scale/256) + bias
 * This method requires heavy optimization since it makes a lot of calculations
*************************************************************************/
void updateMultiplexer()
{
    int32_t v;
    int32_t outv[NUMCHN_TOTAL] = {0};  // must use this since we want to start off with 0 on eack accumulator
                                       // also, since the accumulator is 32bit we do not have to limit it

    //mixer loop
    for(uint8_t i=0; i<NUM_MIXERS; i++)  // mixer loop
    {
        if(!gVars.mixers[i].dest) break; // dest = 0  => end of valid mixers

        MixType * mt = &gVars.mixers[i];
        if(mt->dest>NUMCHN_TOTAL) continue; // invalid value in dest

        if(getVal(mt->swtch))  //if val is nonzero -> on
        {
            v = getVal(mt->source);
            v = FASTDIV_RESX(v*mt->scale) + mt->bias;

            switch (mt->mltpx)
            {
            case (MLTPX_SUB):
                outv[mt->dest-1] -= v; // subtract from to output
                break;
            case (MLTPX_MUL):
                outv[mt->dest-1] = FASTDIV_RESX(v*outv[mt->dest-1]); // subtract from to output
                break;
            case (MLTPX_DIV):
                if(outv[mt->dest-1]) outv[mt->dest-1] = (v*RESX/outv[mt->dest-1]); // subtract from to output
                break;
            case (MLTPX_REP):
                outv[mt->dest-1] = v; // replace output
                break;
            default:
                outv[mt->dest-1] += v; // add to output
                break;
            }
        }
    }

    for(uint8_t i=0; i<NUMCHN_TOTAL; i++) // make sure we don't get noise from runoff
        outval[i] = INTMINMAX(outv[i]);

    //limits loop
    for(uint8_t i=0; i<NUMCHN_OUT; i++)
    {
        LimitsType * lt = &gVars.limits[i];
        v = outval[i];

        int16_t ofs = lt->bias;
        if(ofs>lt->max) ofs = lt->max;
        if(ofs<lt->min) ofs = lt->min;

        //interpolate values so we get a continuous motion between min -> bias -> max
        if(v) v = (v>0) ?
                    FASTDIV_RESX( v*((int32_t)lt->max-ofs)) :
                    FASTDIV_RESX(-v*((int32_t)lt->min-ofs)) ; // divide by RESX  this assumes max/min are +-1024 (RESX)

        //add bias, invert, limit and output to PWM
        v += ofs;
        if(lt->inv) v = -v;
        if(v>lt->max)
            pwm_set(i+1, lt->max);
        else if(v<lt->min)
            pwm_set(i+1, lt->min);
        else
            pwm_set(i+1, (int16_t)v);
    }
}
